'''
####
Date : 2025-1-8
Notes: UI界面显示
此脚本主要功能:
1. 界面设计-手动控制-状态监控-机器视觉-人机交互
2. 数据交互:机械手控制-示教, Modbus寄存器值进行变量监控
3. 机器视觉:原图显示-结果图显示
4. 人机交互

####
'''

# -*- coding: utf-8 -*-
import logging
import os
import sys
import cv2
import socket
import struct
import time
import modbus_tk.defines as cst
import modbus_tk.modbus_tcp as modbus_tcp
import psutil
from threading import Thread

### 导入Qt的封装函数类（python版本）
from PyQt5 import uic, QtCore
from PyQt5.QtWidgets import QApplication, QMainWindow, QHeaderView
from PyQt5.QtGui import QFont, QImage, QPixmap, QIcon
from PyQt5.QtCore import Qt, QTimer

from PyQt5.QtWidgets import QApplication
from PyQt5.QtCore import QAbstractTableModel, Qt

#### 导入相关类函数-- tcp通讯（modbus地址客户端）-- RTU通讯（拓展板IO状态交互）
#### 导入相关类函数-- driver_move（ZMove上位机通讯）-- visionCapture_（机器视觉-图像获取、图像处理类）
from processModbusTCP__ import process_Master
from processModbusRTU_ import rtu_Master
from driver_move import Controler_Server
from visionCapture_ import Camera_Thread

### 初始化对象--线程（避免主线程卡顿）
server = Controler_Server()
ser_thread = Thread(target=server.dorun)
ser_thread.start()

### 日志文件
robot_logger=logging.getLogger("ZMove")
robot_logger.setLevel(logging.DEBUG)
### 默认显示
os.environ['DISPLAY'] = ':0'

#### Z_Move示教软件复制粘贴此处，方便人机交互时候调用点位
P1=[84.625,63.25,106.6,18.84,0.0,0.0,13.116,139.405,57.849,0.0,0.0,103.465,0.0]
P2=[88.1,65.475,76.725,15.363,0.0,0.0,7.112,214.399,50.854,0.0,0.0,103.463,0.0]

class MainWindow(QMainWindow):

#### 用户自己修改人机交互控制部分
#### 人机交互-------------------------开始 ####

    #### 控制机器人动作的任务判断（用户自定义）-----********------重要代码部分
    #### 控制机器人动作的任务判断（用户自定义）-----********------重要代码部分
    #### 控制机器人动作的任务判断（用户自定义）-----********------重要代码部分
    ## 机器人A运动， com_Num = 1
    def robotTask(self, task_id):
        try:
            if task_id == 1:
                #### Note: 由于机器人为单数据通道，后台不停获取数据更新
                #### 故而：利用线程中的类，对机器人的控制进行点动操作
                #### set_joint_angle为机器人轴角运动控制API，可自行查看实现
                server.vsrserver.robot_control.home_position(com_Num=1)

            elif task_id == 2:
                #### Note: 由于控制器连接多台机器人，所以在进行复位操作时
                ####       选择新的API，指定端口号进行相应机器人控制操作
                
                ### ---------------两种方式获取DI外部输入状态---------------------
                ###直接通过rtu获取扩展板的外部输入状态 0-10（DI）11-19（DO）20（motor）
                DI = rtu_master.rtu_server.execute(1, cst.READ_HOLDING_REGISTERS, starting_address= 0, quantity_of_x= 11)
                ###或者，读取寄存器值
                # DI = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address= 37, quantity_of_x= 11)
                while DI[0] == 1:
                    server.vsrserver.robot_control.set_joint_angle(com_P = P1, com_Num=1)
            
            elif task_id == 3:
                #### Note: 由于控制器连接多台机器人，所以在进行复位操作时
                ####       选择新的API，指定端口号进行相应机器人控制操作
                server.vsrserver.robot_control.set_joint_angle(com_P = P2, com_Num=1)

            elif task_id == 4:
                #### Note: 由于控制器连接多台机器人，所以在进行复位操作时
                ####       选择新的API，指定端口号进行相应机器人控制操作
                server.vsrserver.robot_control.set_joint_angle(com_P = P1, com_Num=1)
                server.vsrserver.robot_control.set_joint_angle(com_P = P2, com_Num=1)
                server.vsrserver.robot_control.set_joint_angle(com_P = P1, com_Num=1)
            
        except BaseException as e:
            robot_logger.error(e,exc_info=True)


    #### 控制机器人动作的任务判断（用户自定义第二行）
    ## 机器人B运动
    def robotTask2(self, task_id):
        try:
            if task_id == 1:
                server.vsrserver.robot_control.set_joint_angle(com_P = P1, com_Num=2)
                server.vsrserver.robot_control.set_joint_angle(com_P = P2, com_Num=2)
                server.vsrserver.robot_control.set_joint_angle(com_P = P1, com_Num=2)
            
            elif task_id == 2:
                server.vsrserver.robot_control.home_position(com_Num=2)
            
        except BaseException as e:
            robot_logger.error(e,exc_info=True)

    def autoworking(self):
        try:
            for i in range(1,5):
                self.robotTask(i)
        
        except Exception as e:
            print(f"autoworking error is {e}")

    #### 初始化按钮，以及对应的槽函数（任务ID）
    def taskPushButtonInit(self):
        ## 用户自定义任务按钮
        taskButtons = {
            self.pushButton_taskPage1_task1: 1,
            self.pushButton_taskPage1_task2: 2,
            self.pushButton_taskPage1_task3: 3,
            self.pushButton_taskPage1_task4: 4,
        }
        for button, task_id in taskButtons.items():
            button.clicked.connect(lambda checked, task_id=task_id: self.robotTask(task_id))

        ## 用户自定义任务按钮（第二行）
        taskButtons2 = {
            self.pushButton_taskPage1_task1_2: 1,
            self.pushButton_taskPage1_task2_2: 2,
        }

        for button, task_id in taskButtons2.items():
            button.clicked.connect(lambda checked, task_id=task_id: self.robotTask2(task_id))
        
        ## 页面切换按钮page1,page2
        pageButtons = {
            self.pushButton_taskPage1: 0,
            self.pushButton_taskPage2: 1,
        }

        for button, page_index in pageButtons.items():
            button.clicked.connect(lambda checked, index=page_index: self.stackedWidget_task.setCurrentIndex(index))
        
        ## 界面按钮：急停，机器人A、B复位，自动模式
        uiButtons = {
            self.pushButton_customizeStop: 1,
            self.pushButton_customizeRobotA_Home: 2,
            self.pushButton_customizeRobotB_Home: 3,
            self.pushButton_auto_manually: 4,
        }
        
        for button, task_id in uiButtons.items():
            button.clicked.connect(lambda checked, task_id=task_id: self.robotCMD(task_id))
    

    def robotCMD(self, task_id):
        try:
            robot1_type = server.vsrserver.get_robot_type("robot_type")
            robot2_type = server.vsrserver.get_robot_type("robot_type2")
            if task_id == 1:
                server.vsrserver.robot_control.allRobot_stop()

            elif task_id == 2:
                if robot1_type != "未连接":
                    server.vsrserver.robot_control.home_position(com_Num=1)
            
            elif task_id == 3:
                if robot2_type != "未连接":
                    server.vsrserver.robot_control.home_position(com_Num=2)
            elif task_id == 4:
                self.autoworking()
            
        except BaseException as e:
            robot_logger.error(e,exc_info=True)

#### 人机交互-------------------------结束 ####



    def __init__(self):
        super().__init__()
        self.initParameters()
        self.setup_ui()
        self.msg_history = list()
        self.msgVisionPara_history = list()
        self.msgVisonRes_history = list()
        self.init_pushButtonSignals()
        self.init_labelSingls()
        self.set_initial_size()
        self.robot_control_thread()
        
        ### 机械手信息更新线程
        self.robotInfo_thread = Thread(target=self.init_labelRobotAB)
        self.robotInfo_thread.start()
        self.init_DOpushButton()

        #### 视觉显示线程
        self.camera_thread = Camera_Thread(self.label_oriShow)
        self.camera_thread.start()

        self.cameraUpdate_timer = QTimer(self)
        self.cameraUpdate_timer.timeout.connect(self.visionUpdateFrame)

        ### 视觉显示
        self.vision_initSingal()

        #### 变量监控页面显示
        #### new
        self.dataPageInit()
        self.setup_pages('bool')
        self.setup_pages('int')
        self.setup_pages('float')
        self.setup_pages('pose')

        self.timer = QTimer(self)
        self.timer.timeout.connect(self.valueMonitor)
        self.timer.timeout.connect(self._initIOValuesMonitor)
        self.timer.timeout.connect(self.getModbusClient)
        self.timer.start(1000) 
        
        #### 人机交互页面显示
        ## 自定义任务按钮初始化
        self.taskPushButtonInit()

    ### 初始化参数
    def initParameters(self):
        self.robotA_type = None
        self.robotB_type = None
        ### 机器人运动步进、速度
        self.step = 5
        self.speed = 2000
    
    ### 初始化UI显示位置及尺寸
    def set_initial_size(self):
        self.setGeometry(1, 25, 1280, 640)
    
    ### 加载qt designer文件
    def setup_ui(self):
        current_dir = os.path.dirname(os.path.abspath(__file__))
        ui_file_path = os.path.join(current_dir, "..", "ui", "mainWindow.ui")
        uic.loadUi(ui_file_path, self)
        self.setWindowTitle("ZBot")
        self.setWindowIcon(QIcon('../img/icon.png'))

    ### 初始化UI按钮信号
    def init_pushButtonSignals(self):
        try:
            # Mapping buttons to their corresponding stacked widget indices
            ### 主界面按钮--以及对应的页面
            button_to_page = {
                self.pushButton_tpu: 1,
                self.pushButton_monitor: 2,
                self.pushButton_vision: 3,
                self.pushButton_tcp: 4,
                self.pushButton_comeBack: 0,
                self.pushButton_comeBack_2: 0,
                self.pushButton_comeBack_3: 0,
                self.pushButton_comeBack_4: 0,

            }

            # def pushButtonStyle(page_index):
            #     if page_index == 1 :
            #         self.pushButton_tpu.setStyleSheet("background-color: rgb(255, 129, 66);")
            #     elif page_index == 2:
            #         self.pushButton_monitor.setStyleSheet("background-color:  rgb(16, 200, 255);")
            #     elif page_index == 3 :
            #         self.pushButton_vision.setStyleSheet("background-color: rgb(21, 96, 130);")
            #     elif page_index == 4:
            #         self.pushButton_tcp.setStyleSheet("background-color:  rgb(25, 107, 36);")

            for button, page_index in button_to_page.items():
                button.clicked.connect(lambda checked, index=page_index: self.stackedWidget.setCurrentIndex(index))
                # button.clicked.connect(lambda checked, index=page_index: pushButtonStyle(index))
                if page_index == 1 :
                    self.pushButton_tpu.setStyleSheet("background-color: rgb(255, 129, 66);")
                elif page_index == 2:
                    self.pushButton_monitor.setStyleSheet("background-color:  rgb(16, 200, 255);")
                elif page_index == 3 :
                    self.pushButton_vision.setStyleSheet("background-color: rgb(21, 96, 130);")
                elif page_index == 4:
                    self.pushButton_tcp.setStyleSheet("background-color:  rgb(25, 107, 36);")

                # self.stop_qr_detection()

            
            ### 手动控制页面按钮--对应的机器人示教界面
            robot_button_to_page = {
                self.pushButton_pageArobot: 0,
                self.pushButton_pageBrobot: 1,
            }

            for butt, page_ind in robot_button_to_page.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.stackedWidget_robot.setCurrentIndex(index))
            

            ### 手动控制页面侧面按钮--运动控制--IO控制
            control_button = {
                self.pushButton_robotContorl: 0,
                self.pushButton_ioControl: 1,
            }
            
            for butt, page_ind in control_button.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.stackedWidget_2.setCurrentIndex(index))

            self.pushButton_close.clicked.connect(self.close)
            self.pushButton_close_2.clicked.connect(self.close)
            self.pushButton_close_3.clicked.connect(self.close)
            self.pushButton_close_4.clicked.connect(self.close)

            self.mointor_pushButton()

        except Exception as e:
            print(f"init_pushButtonSingnals error is {e}")

    ### 机器人控制 示教--复位--回零--急停
    def robot_control_thread(self):
        self.robotA_jointTeach()
        self.robotA_coordinateTeach()
        self.robotB_jointTeach()
        self.robotB_coordinateTeach()
        self.cmd_robot()

    ### 初始化UI标签显示
    ######################此方法的更新适用于所有界面的状态显示
    def init_labelSingls(self):
        ### 连接IP地址（本地）
        self.label_IPaddress.setText(self.get_local_ip())
        self.label_IPaddress_2.setText(self.get_local_ip())
        self.label_IPaddress_3.setText(self.get_local_ip())
        self.label_IPaddress_4.setText(self.get_local_ip())
        
        ### KBot软件连接状态
        # print(f"server.vsrserver.KBotConnectStatus {server.vsrserver.KBotConnectStatus}")
        if server.vsrserver.KBotConnectStatus:
            self.label_connectKBot.setText("已连接")
            self.label_connectKBot_2.setText("已连接")
            self.label_connectKBot_3.setText("已连接")
            self.label_connectKBot_4.setText("已连接")
        else:
            self.label_connectKBot.setText("未连接")
            self.label_connectKBot_2.setText("未连接")
            self.label_connectKBot_3.setText("未连接")
            self.label_connectKBot_4.setText("未连接")

    ### 打印视觉结果信息
    def logVisionResult(self, msg):
        msg = time.strftime('%Y-%m-%d %H:%M:%S', time.localtime(time.time())) + " 视觉检测结果: " +  msg
        self.msg_history.append(msg)
        self.textBrowser_visionResultInfo.setText("".join(self.msg_history))
        self.textBrowser_visionResultInfo.moveCursor(self.textBrowser_visionResultInfo.textCursor().End)
        self.textBrowser_visionResultInfo.repaint()

    ### 打印视觉参数信息
    def logVisionParameter(self, msg):
        msg =" 视觉参数: \n" +  msg
        self.msgVisionPara_history.append(msg)
        self.textBrowser_visionParam.setText("".join(self.msgVisionPara_history))
        self.textBrowser_visionParam.moveCursor(self.textBrowser_visionParam.textCursor().End)
        self.textBrowser_visionParam.repaint()
    
    ### 打印modbusClient连接信息
    def logModbusClient(self, msg):
        msg = "\n modBus客户端IP: \n" +  msg
        self.msgVisonRes_history.append(msg)
        self.textBrowser_modbusClient.setText("".join(self.msgVisonRes_history))
        self.textBrowser_modbusClient.moveCursor(self.textBrowser_modbusClient.textCursor().End)
        self.textBrowser_modbusClient.repaint()

    def getModbusClient(self):
        modbus_clients = server.modbusClients
        if modbus_clients is not None:
            for item in modbus_clients:
                self.logModbusClient(str(item))
    
    ### 获取本地IP地址
    def get_local_ip(self):
        for interface, addrs in psutil.net_if_addrs().items():
            for addr in addrs:
                if addr.family == socket.AF_INET and not addr.address.startswith("127."):
                    return addr.address
        return None

    '''
    示例代码：
        1. UI数据绑定
        2. 示教控制代码:412行 手动控制界面-----Start  ---->  887 行 手动控制界面-----End
        3. 状态监控界面： 732行---Start   957行---End
    '''

    ### 数值转换函数--读取寄存器值，转换为float用于显示
    def int2float(self,a,b):
        value=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            value=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
            if value is None:
                return value
            if isinstance(value, float):
                # 精确到小数点后两位数
                return round(value , 2)
            else:
                return value
        except BaseException as e:
            robot_logger.error(e,exc_info=True)
        return value

#############################
####### 手动控制界面-----Start
    ###-- 1. 机器人类型：KT-R6（6轴机器人）， KT-R4
    ###-- 2. 机器人状态： 离线--空闲-- 锁定--复位--运行
    def init_labelRobotAB(self):
        try:
            while True:
                self.robotA_type = server.vsrserver.get_robot_type("robot_type")
                self.robotB_type = server.vsrserver.get_robot_type("robot_type2")
                self.label_robotAtype.setText(self.robotA_type)
                self.label_robotBtype.setText(self.robotB_type)
                self.label_robot1Connect.setText(self.robotA_type)
                self.label_robot2Connect.setText(self.robotB_type)
                self.label_robot1Connect_2.setText(self.robotA_type)
                self.label_robot2Connect_2.setText(self.robotB_type)
                self.label_robot1Connect_3.setText(self.robotA_type)
                self.label_robot2Connect_3.setText(self.robotB_type)
                self.label_robot1Connect_4.setText(self.robotA_type)
                self.label_robot2Connect_4.setText(self.robotB_type)
                self.label_robot1Connect_5.setText(self.robotA_type)
                self.label_robot2Connect_5.setText(self.robotB_type)

                state_val = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, 0, 1)[0]
                if state_val == 0:
                    robotA_state = "离线"
                elif state_val == 1:
                    robotA_state = "空闲"
                elif state_val == 2:
                    robotA_state = "锁定"
                elif state_val == 3:
                    robotA_state = "复位"
                elif state_val == 4:
                    robotA_state = "运行"
                else:
                    robotA_state = "未知"
                
                state_val = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, 60, 1)[0]
                if state_val == 0:
                    robotB_state = "离线"
                elif state_val == 1:
                    robotB_state = "空闲"
                elif state_val == 2:
                    robotB_state = "锁定"
                elif state_val == 3:
                    robotB_state = "复位"
                elif state_val == 4:
                    robotB_state = "运行"

                self.label_robotAstatus.setText(robotA_state)
                self.label_robotBstatus.setText(robotB_state)
                self.label_robotAstatus1.setText(robotA_state)
                self.label_robotBstatus1.setText(robotB_state)
                self.label_robotAstatus2.setText(robotA_state)
                self.label_robotBstatus2.setText(robotB_state)
                self.label_robotAstatus3.setText(robotA_state)
                self.label_robotBstatus3.setText(robotB_state)
                self.label_robotAstatus4.setText(robotA_state)
                self.label_robotBstatus4.setText(robotB_state)

                self.joint_coordinateShow()

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def init_DOpushButton(self):
        self.do_buttons = {
            self.pushButton_toggle_do_0: 0,
            self.pushButton_toggle_do_1: 1,
            self.pushButton_toggle_do_2: 2,
            self.pushButton_toggle_do_3: 3,
            self.pushButton_toggle_do_4: 4,
            self.pushButton_toggle_do_5: 5,
            self.pushButton_toggle_do_6: 6,
            self.pushButton_toggle_do_7: 7,
        }
        
        # Initialize a dictionary to keep track of each button's state.
        self.button_states = {index: False for index in self.do_buttons.values()}

        for butt, page_ind in self.do_buttons.items():
            butt.clicked.connect(lambda checked, index=page_ind: self.on_toggle(index, checked))

        self.motor_buttons = {
            self.pushButton_forward: 1,
            self.pushButton_inverse: 2,
            self.pushButton_Motor_start: 3,
            self.pushButton_Motor_stop:4,
        }
    
        for butt, page_ind in self.motor_buttons.items():
            butt.clicked.connect(lambda checked, index=page_ind: self.on_motorControl(index))

    def on_toggle(self, index, checked):
        # Update the state for the specific button.
        self.button_states[index] = checked
        
        button = [butt for butt, idx in self.do_buttons.items() if idx == index][0]
        # button.setText("ON" if checked else "OFF")
        if checked:
            button.setText("ON")
            button.setStyleSheet("background-color: green;")
            rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = index + 11, output_value= 1)
        else:
            button.setText("OFF")
            button.setStyleSheet("background-color: grey;")
            rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = index + 11, output_value= 0)

        # print(f"Switch {index} is {'ON' if checked else 'OFF'}")

    def on_motorControl(self, index):
        if index == 1:
            rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 20, output_value= 1)
        elif index == 2: 
            rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 20, output_value= 2)
        elif index == 3:
            rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 20, output_value= 1)
        else:
            rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 20, output_value= 0)

    ###-- 3. 机器人坐标轴角、笛卡尔显示： joint_coordinateShow
    ###-- 4. 外层函数_get_robot_pose， 不同机器人取地址不同get_pose_floats， 寄存器读取姿态：read_pose_floats
    def read_pose_floats(self, starting_address):
        motion = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address, 26)
        if not motion:
            return []

        floats = []
        for i in range(0, 26, 2):
            if i + 1 < len(motion):
                floats.append(self.int2float(motion[i], motion[i+1]))
            else:
                print("Error: Not enough integers to convert to floats.")
                break
        return floats

    def get_pose_floats(self, robotNum):
        try:
            if robotNum == 1:
                robot1_pose = self.read_pose_floats(1)
                return robot1_pose
            elif robotNum == 2:
                robot2_pose = self.read_pose_floats(61)
                return robot2_pose

        except Exception as e:
            print(e)

    def _get_robot_pose(self, robot_id):  
        try:  
            pose = self.get_pose_floats(robot_id)  
            return pose 
         
        except Exception as e:  
            return f"Error: {str(e)}"  

    def joint_coordinateShow(self):
        try:
            robot_pose = self._get_robot_pose(1)
            joints = robot_pose[:6]    
            coords = robot_pose[6:12]  
    
            for i, joint in enumerate(joints, 1):  
                getattr(self, f'label_J{i}show').setText(str(joint))
            for i, coord in enumerate(coords, 1):
                getattr(self, f'label_C{i}show').setText(str(coord))
            
            # # 如果机器人有第7个关节 
            # if len(robot_pose) > 12:  
            #     getattr(self, f'label_J7').setText(str(robot_pose[12]))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ###-- 5. A机器人控制：robotA_jointTeach轴角   robotA_coordinateTeach笛卡尔示教
    def robotA_jointTeach(self):
        try:
            joint_Pushbutton = {
                self.pushButton_J1min: 1,
                self.pushButton_J2min: 2,
                self.pushButton_J3min: 3,
                self.pushButton_J4min: 4,
                self.pushButton_J5min: 5,
                self.pushButton_J6min: 6,
                self.pushButton_J1add: 7,
                self.pushButton_J2add: 8,
                self.pushButton_J3add: 9,
                self.pushButton_J4add: 10,
                self.pushButton_J5add: 11,
                self.pushButton_J6add: 12,
            }
            
            # for butt, page_ind in joint_Pushbutton.items():
            #     butt.clicked.connect(lambda checked, index=page_ind: self.jointControRobotA(index))
            for butt, page_ind in joint_Pushbutton.items():
                # 连接 pressed 信号到第一个函数
                butt.pressed.connect(lambda checked=False, index=page_ind: self.jointControRobotA(index))
                # 连接 released 信号到第二个函数
                butt.released.connect(lambda checked=False, index=page_ind: self.jointControRobotRelease(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def jointControRobotA(self,index):
        try:
            speed = 2000
            s_data = server.vsrserver.robot_control.status_global
            angle_current = s_data["angle"]
            angle_current = [int(i) for i in angle_current]
            if self.robotA_type == "KT-R6":
                ### 1 2 3 4 5 6 轴角 减  7 8 9 10 11 12 轴角 加  21 复位  21 回零 23 急停
                if index == 1:
                    gcode = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 2:
                    gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 3:
                    gcode = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 4:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[4],angle_current[5], speed)
                elif index == 5:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B-180 C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[3],angle_current[5], speed)
                elif index == 6:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[3],angle_current[4], speed)
                elif index == 7:
                    gcode = "M21 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 8:
                    gcode = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 9:
                    gcode = "M21 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 10:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A140 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[4],angle_current[5], speed)
                elif index == 11:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[3],angle_current[5], speed)
                elif index == 12:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[3],angle_current[4], speed)

            elif self.robotA_type == "KT-R4":
                ### 1 2 3 4 5 6 轴角 减  7 8 9 10 11 12 轴角 加  21 复位  21 回零 23 急停
                if index == 1:
                    gcode = "M21 G90 G01 X-130 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 2:
                    gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 3:
                    gcode = "M21 G90 G01 X{} Y{} Z-10 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 4:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[4],angle_current[5], speed)
                elif index == 7:
                    gcode = "M21 G90 G01 X180 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 8:
                    gcode = "M21 G90 G01 X{} Y45 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 9:
                    gcode = "M21 G90 G01 X{} Y{} Z40 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 10:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[4],angle_current[5], speed)
                
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
            

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def jointControRobotRelease(self,index):
        try:
            # gcode = "o117\r\n"
            gcode = "!\r\n"
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
            time.sleep(0.5)
            gcode = f"%\n"
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def robotA_coordinateTeach(self):
        try:
            coordinate_PushbuttonA = {
                self.pushButton_C1min: 1,
                self.pushButton_C2min: 2,
                self.pushButton_C3min: 3,
                self.pushButton_C4min: 4,
                self.pushButton_C5min: 5,
                self.pushButton_C6min: 6,
                self.pushButton_C1add: 7,
                self.pushButton_C2add: 8,
                self.pushButton_C3add: 9,
                self.pushButton_C4add: 10,
                self.pushButton_C5add: 11,
                self.pushButton_C6add: 12,

            }

            for butt, page_ind in coordinate_PushbuttonA.items():
                # butt.clicked.connect(lambda checked, index=page_ind: self.coordControRobotA(index))
                # 连接 pressed 信号到第一个函数
                butt.pressed.connect(lambda checked=False, index=page_ind: self.coordControRobotA(index))
                # 连接 released 信号到第二个函数
                butt.released.connect(lambda checked=False, index=page_ind: self.coordControRobotRelease(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def coordControRobotA(self,index):
        try:
            speed = self.speed
            s_data = server.vsrserver.robot_control.status_global
            angle_current = s_data["angle"]
            angle_current = [int(i) for i in angle_current]
            ### 1 2 3 4 5 6 笛卡尔 减  7 8 9 10 11 12 笛卡尔 加
            if index == 1:
                gcode = f"M20 G91 G01 X-{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
            elif index == 2:
                gcode = f"M20 G91 G01 X0 Y-{self.step} Z0 A0 B0 C00 F{speed}\n"
            elif index == 3:
                gcode = f"M20 G91 G01 X0 Y0 Z-{self.step} A0 B0 C00 F{speed}\n"
            elif index == 4:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A-{self.step} B0 C00 F{speed}\n"
            elif index == 5:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B-{self.step} C00 F{speed}\n"
            elif index == 6:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C-{self.step} F{speed}\n"
            elif index == 7:
                gcode = f"M20 G91 G01 X{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
            elif index == 8:
                gcode = f"M20 G91 G01 X0 Y{self.step} Z0 A0 B0 C00 F{speed}\n"
            elif index == 9:
                gcode = f"M20 G91 G01 X0 Y0 Z{self.step} A0 B0 C00 F{speed}\n"
            elif index == 10:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A{self.step} B0 C00 F{speed}\n"
            elif index == 11:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B{self.step} C00 F{speed}\n"
            elif index == 12:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C{self.step} F{speed}\n"            
            
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def coordControRobotRelease(self,index):
        try:
            # gcode = "o117\r\n"
            gcode = "!\r\n"
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
            time.sleep(0.5)
            gcode = f"%\n"
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ###-- 6. B机器人控制：robotB_jointTeach轴角   robotB_coordinateTeach笛卡尔示教
    def robotB_jointTeach(self):
        try:
            joint_PushbuttonB = {
                self.pushButton_J1min_B: 1,
                self.pushButton_J2min_B: 2,
                self.pushButton_J3min_B: 3,
                self.pushButton_J4min_B: 4,
                self.pushButton_J5min_B: 5,
                self.pushButton_J6min_B: 6,
                self.pushButton_J1add_B: 7,
                self.pushButton_J2add_B: 8,
                self.pushButton_J3add_B: 9,
                self.pushButton_J4add_B: 10,
                self.pushButton_J5add_B: 11,
                self.pushButton_J6add_B: 12,
            }
            for butt, page_ind in joint_PushbuttonB.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.jointControRobotB(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def jointControRobotB(self,index):
        try:
            speed = 2000
            s_data_ = server.vsrserver.robot_control.status_global_
            angle_current = s_data_["angle"]
            angle_current = [int(i) for i in angle_current]
            ### 1 2 3 4 5 6 轴角 减  7 8 9 10 11 12 轴角 加
            if index == 1:
                gcode = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 2:
                gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 3:
                gcode = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 4:
                gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[4],angle_current[5], speed)
            elif index == 5:
                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B-180 C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[3],angle_current[5], speed)
            elif index == 6:
                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[3],angle_current[4], speed)
            elif index == 7:
                gcode = "M21 G90 G01 X100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 8:
                gcode = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 9:
                gcode = "M21 G90 G01 X{} Y{} Z120 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 10:
                gcode = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[4],angle_current[5], speed)
            elif index == 11:
                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[3],angle_current[5], speed)
            elif index == 12:
                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[3],angle_current[4], speed)

            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)
            

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def robotB_coordinateTeach(self):
        try:
            coordinate_PushbuttonB = {
                self.pushButton_C1min_B: 1,
                self.pushButton_C2min_B: 2,
                self.pushButton_C3min_B: 3,
                self.pushButton_C4min_B: 4,
                self.pushButton_C5min_B: 5,
                self.pushButton_C6min_B: 6,
                self.pushButton_C1add_B: 7,
                self.pushButton_C2add_B: 8,
                self.pushButton_C3add_B: 9,
                self.pushButton_C4add_B: 10,
                self.pushButton_C5add_B: 11,
                self.pushButton_C6add_B: 12,
            }

            for butt, page_ind in coordinate_PushbuttonB.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.coordControRobotB(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def coordControRobotB(self,index):
        try:
            speed = self.speed
            s_data_ = server.vsrserver.robot_control.status_global_
            angle_current = s_data_["angle"]
            angle_current = [int(i) for i in angle_current]
            ### 1 2 3 4 5 6 笛卡尔 减  7 8 9 10 11 12 笛卡尔 加
            if index == 1:
                gcode = f"M20 G91 G01 X-{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
            elif index == 2:
                gcode = f"M20 G91 G01 X0 Y-{self.step} Z0 A0 B0 C00 F{speed}\n"
            elif index == 3:
                gcode = f"M20 G91 G01 X0 Y0 Z-{self.step} A0 B0 C00 F{speed}\n"
            elif index == 4:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A-{self.step} B0 C00 F{speed}\n"
            elif index == 5:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B-{self.step} C00 F{speed}\n"
            elif index == 6:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C-{self.step} F{speed}\n"
            elif index == 7:
                gcode = f"M20 G91 G01 X{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
            elif index == 8:
                gcode = f"M20 G91 G01 X0 Y{self.step} Z0 A0 B0 C00 F{speed}\n"
            elif index == 9:
                gcode = f"M20 G91 G01 X0 Y0 Z{self.step} A0 B0 C00 F{speed}\n"
            elif index == 10:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A{self.step} B0 C00 F{speed}\n"
            elif index == 11:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B{self.step} C00 F{speed}\n"
            elif index == 12:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C{self.step} F{speed}\n"            
            
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ###-- 7. 复位 急停 回零（命令机械臂）
    def cmd_robot(self):
        try:
            cmd_pushButton = {
                self.pushButton_robotHome:21,
                self.pushButton_robotZero:22,
                self.pushButton_robotStop:23,
            }

            cmd_pushButtonB = {
                self.pushButton_robotHome_B:31,
                self.pushButton_robotZero_B:32,
                self.pushButton_robotStop_B:33,
            }
            for butt, page_ind in cmd_pushButton.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.cmd_robotControl(index))
            
            for butt, page_ind in cmd_pushButtonB.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.cmd_robotControlB(index))
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def cmd_robotControl(self,index):
        try:
                if index == 21:
                    gcode = "$h\r\n"
                elif index == 22:
                    gcode = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000\r\n'
                elif index == 23:
                    gcode = "o117\r\n"

                server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)

        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def cmd_robotControlB(self,index):
        try:
                if index == 31:
                    gcode = "$h\r\n"
                elif index == 32:
                    gcode = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000\r\n'
                elif index == 33:
                    gcode = "o117\r\n"

                server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)

        except Exception as e:
            robot_logger.error(e,exc_info=True)


####### 手动控制界面------End


##############################
####### 状态监控界面------Start

    #### 0. 按钮初始化
    def mointor_pushButton(self):
        try:
            self.textBrowser_modbusClient.setMaximumHeight(60)  # 设定固定的打印高度
            self.textBrowser_modbusClient.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)  # 禁用垂直滚动条
            self.pushButton_modbusClientClear.clicked.connect(self.clearModbusClients)

            buttons = {
                self.pushButton_bool: 0,
                self.pushButton_int: 1,
                self.pushButton_float: 2,
                self.pushButton_pose: 3,
                self.pushButton_io:4,
            }
            for button, page_index in buttons.items():
                ### 设置默认样式
                default_style = "QPushButton { background-color: white; }"
                active_style = "QPushButton { background-color: lightblue; }"

                button.setStyleSheet(default_style)

                def on_click(checked, index=page_index, btn=button):
                    ### 当按钮被点击时，改变当前索引并更新样式
                    self.stackedWidget_mointor.setCurrentIndex(index)
                    ### 移除所有按钮的激活样式
                    for b in buttons:
                        b.setStyleSheet(default_style)
                    ### 给被点击的按钮应用激活样式
                    btn.setStyleSheet(active_style)

                ### 连接按钮点击信号到槽函数
                button.clicked.connect(on_click)
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ###-- 1-4. IO型变量监控
    def _initIOValuesMonitor(self):
        try:
            for i in range (0,8):
                do_is_open = process_master.get_All_DO(i)
                # label_name = f'DO{i}'
                label_name = f'label_light_do{i}_2'
                label = getattr(self, label_name, None)
                if label is None:
                    print(f"Warning: {label_name} not found.")
                    continue
                if do_is_open:
                    label.setStyleSheet("background-color: green; border-radius: 10px;")
                else:
                    label.setStyleSheet("background-color: gray; border-radius: 10px;")

            for i in range (0,10):
                di_is_open = process_master.get_All_DI(i)
                label_di_name = f'label_light_di{i}_2'
                label_di = getattr(self, label_di_name, None)
                if label_di is None:
                    print(f"Warning: {label_di_name} not found.")
                    continue
                if di_is_open:
                    label_di.setStyleSheet("background-color: green; border-radius: 10px;")
                else:
                    label_di.setStyleSheet("background-color: gray; border-radius: 10px;")

            motor = process_master.get_Motor(0)
            if motor == 0:
                self.pushButton_Motor_stop_2.setChecked(True)
                self.pushButton_forward_2.setChecked(False)
                self.pushButton_inverse_2.setChecked(False)
            elif motor == 1:
                self.pushButton_forward_2.setChecked(True)
                self.pushButton_Motor_stop_2.setChecked(False)
                self.pushButton_inverse_2.setChecked(False)
            elif motor ==2:
                self.pushButton_inverse_2.setChecked(True)
                self.pushButton_forward_2.setChecked(False)
                self.pushButton_Motor_stop_2.setChecked(False)

        except Exception as e:
            print(f"Error: {str(e)}")
    
    def clearModbusClients(self):
        self.msgVisonRes_history.clear()
        self.textBrowser_modbusClient.clear()

    ### 1. 初始化按钮、视图等信息
    def dataPageInit(self):
        self.table_views = {
            'bool': [self.tableView_bool1_1, self.tableView_bool1_2, self.tableView_bool1_3,
                    self.tableView_bool2_1, self.tableView_bool2_2, self.tableView_bool2_3],
            'int': [self.tableView_int1_1, self.tableView_int1_2, self.tableView_int1_3,
                    self.tableView_int2_1, self.tableView_int2_2, self.tableView_int2_3],
            'float': [self.tableView_float1_1, self.tableView_float1_2, self.tableView_float1_3,
                    self.tableView_float2_1, self.tableView_float2_2, self.tableView_float2_3],
            'pose': [self.tableView_pose1_1, self.tableView_pose1_2, self.tableView_pose1_3,
                    self.tableView_pose2_1, self.tableView_pose2_2, self.tableView_pose2_3]
        }

        self.page_buttons = {
            'bool': (self.pushButton_bool_pageup, self.pushButton_bool_pagedown, self.stackedWidget_boolDataPage),
            'int': (self.pushButton_int_pageup, self.pushButton_int_pagedown, self.stackedWidget_intDataPage),
            'float': (self.pushButton_float_pageup, self.pushButton_float_pagedown, self.stackedWidget_floatDataPage),
            'pose': (self.pushButton_pose_pageup, self.pushButton_pose_pagedown, self.stackedWidget_poseDataPage)
        }

        self.data_generators = {
            'bool': self.create_bool_table_data,
            'int': self.create_int_table_data,
            'float': self.create_float_table_data,
            'pose': self.create_pose_table_data
        }
    
    ### 1.2 设置成功页面
    def setup_pages(self, data_type):
        try:
            # self._timerUpdateModbusValue_new()
            page_up, page_down, stacked_widget = self.page_buttons[data_type]

            # Connect the page buttons to change the current index of the stacked widget
            for butt, index in zip([page_up, page_down], [0, 1]):
                butt.clicked.connect(lambda checked, idx=index: stacked_widget.setCurrentIndex(idx))

            # Setup table views with corresponding data
            start, step = 1, 7
            for table_view in self.table_views[data_type]:
                self.setup_table_view(table_view, self.data_generators[data_type], start, start + step - 1)
                start += step
        
        except Exception as e:
            print(f"setup_pages error is {e}")

    ### 1.2.1 创建bool视图表格
    def create_bool_table_data(self, start, end):
        return [[str(i), f'GB{i:02d}', f'0X_{(i-1):04d}', 'False'] for i in range(start, end+1)]
    
    ### 1.2.2 创建int视图表格
    def create_int_table_data(self, start, end):
        return [[str(i), f'GI{i:02d}', f'4X_{(300 + (i-1)):04d}', '0'] for i in range(start, end+1)]

    ### 1.2.3 创建float视图表格
    def create_float_table_data(self, start, end):
        return [[str(i), f'GF{i:02d}', f'4X_{(400 + (i-1)*2):04d}', '0.0'] for i in range(start, end+1)]
    
    ### 1.2.4 创建pose视图表格
    def create_pose_table_data(self, start, end):
        return [[str(i), f'GP{i:02d}', f'4X_{(500 + (i-1)*2):04d}', '00.00'] for i in range(start, end+1)]

    ### 1.3 初始化填充视图表格，单元格格式等
    def setup_table_view(self, table_view, data_generator, start, end):
        try:
            """设置指定范围的tableView"""
            table_data = data_generator(start, end)
            model = MyTableModel(table_data)
            table_view.setModel(model)

            horizontal_header = table_view.horizontalHeader()
            horizontal_header.setSectionResizeMode(0, QHeaderView.Fixed)
            horizontal_header.resizeSection(0, 46)
            horizontal_header.resizeSection(1, 100)
            horizontal_header.resizeSection(2, 160)
            horizontal_header.resizeSection(3, 76)

            ### 自适应列宽
            # for i in range(1, horizontal_header.count()):
            #     horizontal_header.setSectionResizeMode(i, QHeaderView.Stretch)

            font = QFont("Arial", 18, QFont.Normal)
            table_view.setFont(font)

            header_font = QFont("Arial", 18, QFont.Bold)
            horizontal_header.setFont(header_font)
            horizontal_header.setStyleSheet("::section{height:70px;}")

            QTimer.singleShot(0, lambda tv=table_view: adjust_row_heights(tv))
            table_view.show()
        
        except Exception as e:
            print(f"setup_table_view error is {e}")

    ### 1.4 设置定时器以定期更新值(500ms/次)

    ### 1.5 变量数据监控
    def valueMonitor(self):
        modbus_addr_prefix = ["0X", "4X"]
        self.update_data_from_modbus(modbus_addr_prefix)    

    #### 1.6 """从Modbus设备更新数据"""
    def update_data_from_modbus(self, prefixes):
        data = self.read_modbus_register(prefixes)
        for table_type in ['bool', 'int', 'float', 'pose']:
            self.update_table_view_data(table_type, data)

    ### 1.7 """根据类型更新tableView的数据"""
    def update_table_view_data(self, data_type, data):
        start, step = 1, 7
        for table_view in self.table_views[data_type]:
            model = table_view.model()
            if model is not None:
                for row in range(model.rowCount()):
                    index = model.index(row, 3)  # 第4列索引
                    register_address = self.get_modbus_address(data_type, start + row - 1)
                    
                    # # 对于 float 和 pose 类型，确保只使用偶数地址作为键
                    # if data_type in ['float', 'pose']:
                    #     register_address = register_address if register_address % 2 == 0 else register_address - 1

                    prefix = "4X" if data_type in ['int', 'float', 'pose'] else "0X"
                    key = f"{prefix}_{register_address:04d}"  # 确保地址有四位数

                    if key in data:
                        value = data[key]
                        
                        # 根据值类型格式化显示
                        if isinstance(value, bool):
                            display_value = "False" if not value else "True"
                        elif isinstance(value, int):
                            display_value = str(value)
                        elif isinstance(value, float):
                            display_value = f"{value:.2f}"
                        else:
                            display_value = str(value)

                        model.setData(index, display_value, Qt.EditRole)
                        ## 尝试设置数据并检查是否成功 
                        # if model.setData(index, display_value, Qt.EditRole):
                        #     print(f"Data successfully set for index {index} to {display_value}")
                        # else:
                        #     print(f"Failed to set data for index {index}")
                    
                    # else:
                    #     print(f"Key {key} not found in data.")
                
                # 强制刷新视图
                model.layoutAboutToBeChanged.emit()
                model.layoutChanged.emit()
                table_view.viewport().update()
            
            start += step

    ### 1.8 读取寄存器地址的值
    def read_modbus_register(self, prefixes):
        try:
            data = {}
            for prefix in prefixes:
                if prefix == "0X":
                    pass
                    # 读取线圈（布尔值）
                    # response = process_master.master_.execute(1, cst.READ_COILS, starting_address=0, quantity_of_x=42)
                    # data.update({f"{prefix}_{i:04d}": bool(response[i]) for i in range(len(response))})
                elif prefix == "4X":
                    value_tmp = {
                        'int': 300,
                        'float': 400,
                        'pose': 500
                    }
                    quantities = {
                        'int': 42,
                        'float': 82,
                        'pose': 82
                    }

                    for var_type, start_addr in value_tmp.items():
                        response = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=start_addr, quantity_of_x=quantities[var_type])
                        
                        if var_type == 'int':
                            data.update({f"{prefix}_{start_addr + i:04d}": response[i] for i in range(len(response))})
                        elif var_type in ['float', 'pose']:
                            for i in range(0, len(response), 2):  # 浮点数和位姿占用两个寄存器
                                high_word = response[i]
                                low_word = response[i + 1]
                                float_value = struct.unpack('>f', struct.pack('>HH', high_word, low_word))[0]
                                data[f"{prefix}_{start_addr + i:04d}"] = float_value
            return data
        
        except Exception as e:
            print(f"read_modbus_register error is {e}")

    ### 1.9 """根据类型和索引计算Modbus寄存器地址"""
    def get_modbus_address(self, data_type, register_index):
        """根据类型和索引计算Modbus寄存器地址"""
        base_address = {
            'bool': 0,
            'int': 300,
            'float': 400,
            'pose': 500
        }
        
        # 定义每种类型的寄存器数量
        register_count = {
            'bool': 42,
            'int': 42,
            'float': 42,  # 注意：这是浮点数的数量，不是寄存器数量
            'pose': 42    # 同上
        }

        if register_index > register_count[data_type]:
            raise ValueError(f"Register index {register_index} out of range for {data_type}")

        address_offset = register_index
        if data_type in ['float', 'pose']:
            address_offset *= 2  # 因为浮点数和位姿占用两个寄存器
        
        return base_address[data_type] + address_offset

        # def get_modbus_address(self, data_type, register_index):

        #     base_address = {
        #         'bool': 0,
        #         'int': 300,
        #         'float': 400,
        #         'pose': 500
        #     }
        #     address_offset = register_index - 1
        #     if data_type in ['float', 'pose']:
        #         address_offset *= 2  # 因为浮点数和位姿占用两个寄存器
        #     return base_address[data_type] + address_offset

    ####### 状态监控界面------End


##############################
####### 机器视觉界面------Start
    ### 1. 视觉按钮初始化信号与槽
    def vision_initSingal(self):
        self.textBrowser_visionParam.setMaximumHeight(504)  # 设定固定的打印高度
        self.textBrowser_visionParam.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)  # 禁用垂直滚动条
        self.textBrowser_visionResultInfo.setMaximumHeight(72)  
        self.textBrowser_visionResultInfo.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff) 

        self.pushButton_visionProcess.clicked.connect(self.trigger_qr_detection)
        self.pushButton_visionCapture.clicked.connect(self.trigger_capture)
        self.pushButton_visionAutoManu.clicked.connect(self.autoUpdate)
        self.pushButton_clearPatameter.clicked.connect(self.clearVisionParameter)
        self.pushButton_clearResult.clicked.connect(self.clearVisionParameter)

    """触发二维码识别"""
    def trigger_qr_detection(self):
        self.camera_thread.detect_qr_code = True
        self.visionUpdateFrame()
    
    """停止二维码识别"""
    def stop_qr_detection(self):
        self.camera_thread.detect_qr_code = False

    def trigger_capture(self):
        self.pauseUpdate()
        self.camera_thread.stop = False
        time.sleep(0.5)
        self.visionUpdateFrame()
        self.camera_thread.stop = True

    ## 视觉显示--定时器用于定期检查队列中的新图像(不停刷新--本应用中有拍照，手动模式为拍一张)
    def autoUpdate(self):
        self.camera_thread.stop = False
        if not self.cameraUpdate_timer.isActive():
            self.cameraUpdate_timer.start(50)
    
    ### 暂停定时器
    def pauseUpdate(self):
        self.camera_thread.stop = True
        if self.cameraUpdate_timer.isActive():
            self.cameraUpdate_timer.stop()
    
    ### 恢复定时器
    def resumeUpdate(self):
        if not self.cameraUpdate_timer.isActive():
            self.autoUpdate()


    def closeEvent(self, event):
        self.camera_thread.exit()
        self.camera_thread.join()
        event.accept()

    ### 2. 视觉图像更新
    def visionUpdateFrame(self):
        try:
            ### 更新原始图像
            if not self.camera_thread.raw_frame_queue.empty():
                original_img = self.camera_thread.raw_frame_queue.get_nowait()
                original_img = cv2.resize(original_img, (680, 400))
                rgb_original = cv2.cvtColor(original_img, cv2.COLOR_BGR2RGB)
                h, w, ch = rgb_original.shape
                bytes_per_line = ch * w
                qimg_original = QImage(rgb_original.data, w, h, bytes_per_line, QImage.Format_RGB888)
                self.label_oriShow.setAlignment(Qt.AlignCenter)
                self.label_oriShow.setPixmap(QPixmap.fromImage(qimg_original))

                if self.camera_thread.resultInfo is not None:
                    msg = self.camera_thread.resultInfo
                    self.logVisionResult(msg)
                
                if self.camera_thread.parameterInfo is not None:
                    for item in self.camera_thread.parameterInfo:
                        self.logVisionParameter(str(item))

            # ### 更新处理后的图像（如果有）
            # if not self.camera_thread.processed_frame_queue.empty():
            #     result_img = self.camera_thread.processed_frame_queue.get_nowait()
            #     result_img = cv2.resize(result_img, (600, 500))
            #     rgb_result = cv2.cvtColor(result_img, cv2.COLOR_BGR2RGB)
            #     h, w, ch = rgb_result.shape
            #     bytes_per_line = ch * w
            #     qimg_result = QImage(rgb_result.data, w, h, bytes_per_line, QImage.Format_RGB888)
            #     self.label_resultShow.setAlignment(Qt.AlignCenter)
            #     self.label_resultShow.setPixmap(QPixmap.fromImage(qimg_result))

        except Exception as e:
            print(f"Error visionUpdateFrame: {e}")


    def clearVisionParameter(self):
        self.msgVisionPara_history.clear()
        self.textBrowser_visionParam.clear()

    def clearVisionResult(self):
        self.msgVisonRes_history.clear()
        self.textBrowser_visionResultInfo.clear()

####### 机器视觉界面------End




####  创建表格视图类
class MyTableModel(QAbstractTableModel):
    
    def __init__(self, data=None):
        super().__init__()
        self._data = data or []
        self.headers = ["No.", "变量名", "Modbus地址", "值"]

    def rowCount(self, parent=None):
        return len(self._data)

    def columnCount(self, parent=None):
        return len(self.headers)

    def data(self, index, role=Qt.DisplayRole):
        if not index.isValid():
            return None
        if role == Qt.DisplayRole:
            row = index.row()
            col = index.column()
            if row < len(self._data) and col < len(self._data[row]):
                return self._data[row][col]
        elif role == Qt.TextAlignmentRole:
            return Qt.AlignCenter
        return None

    ### 表格视图-插入数据
    def setData(self, index, value, role=Qt.EditRole):
        if not index.isValid():
            return False
        if role == Qt.EditRole:
            self._data[index.row()][index.column()] = value
            self.dataChanged.emit(index, index, [role])
            return True
        return False

    def headerData(self, section, orientation, role=Qt.DisplayRole):
        if role == Qt.DisplayRole and orientation == Qt.Horizontal:
            return self.headers[section]
        return None

def adjust_row_heights(view):
    for row in range(view.model().rowCount()):
        view.setRowHeight(row, 45) ## 单位：像素



if __name__ == '__main__':

    process_master = process_Master()
    rtu_master = rtu_Master()
    app = QApplication(sys.argv)
    ex = MainWindow()
    # ex.show()
    ex.showMaximized()
    sys.exit(app.exec_())
